#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;
using namespace std;

vector<string> split(const string &line, char delimiter)
{
    vector<string> tokens;
    string token;
    istringstream tokenStream(line);
    while (getline(tokenStream, token, delimiter))
    {
        tokens.push_back(token);
    }
    return tokens;
}

int main()
{
    string filename = "../traj/data.txt";
    string slam = "[plan]";
    int flag_index = 0;
    int x_index = 2;
    int y_index = 4;

    // read data
    ifstream infile(filename);

    if (!infile.is_open())
    {
        cerr << "can not open file" << filename << endl;
        return 1;
    }

    // deal data
    string line;
    vector<double> x1, y1, x2, y2;
    while (getline(infile, line))
    {
        vector<string> tokens = split(line, ' ');
        if (slam.compare(tokens[flag_index]) == 0)
        {
            x1.push_back(stod(tokens[x_index]));
            y1.push_back(stod(tokens[y_index]));
        }
        else
        {
            x2.push_back(stod(tokens[x_index]));
            y2.push_back(stod(tokens[y_index]));
        }
    }

    // show
    plt::subplot(2, 1, 1);
    plt::plot(x1, y1, "-.b");
    plt::subplot(2, 1, 2);
    plt::plot(x2, y2, "-.r");
    plt::legend();
    plt::show();
}
